#include <omplapp/apps/SE3RigidBodyPlanning.h>
#include <omplapp/config.h>
#include "RRTIterationCount.h"
#include "pRRTIterationCount.h"
#include <ompl/base/samplers/UniformValidStateSampler.h>
#include "PoissonRRT.h"
#include "PoissonSampler.h"

using namespace ompl;

int main()
{
	app::SE3RigidBodyPlanning setup;

	// load the robot and the environment
	std::string robot_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/3D/Easy_robot.dae";
	std::string env_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/3D/Easy_env.dae";
	setup.setRobotMesh(robot_fname.c_str());
	setup.setEnvironmentMesh(env_fname.c_str());

	// define starting & goal state
	base::ScopedState<base::SE3StateSpace> start(setup.getSpaceInformation());
	base::ScopedState<base::SE3StateSpace> goal(start);

	// random polygons
	start->setX(270);
	start->setY(160);
	start->setZ(-200);
	start->rotation().setIdentity();
	goal->setX(270);
	goal->setY(160);
	goal->setZ(-400);
	goal->rotation().setIdentity();

	base::RealVectorBounds bounds(3);
	bounds.setHigh(0, 457.960449219);
	bounds.setHigh(1, 321.25);
	bounds.setHigh(2, -72.8550872803);
	bounds.setLow(0, 14.4604492188);
	bounds.setLow(1, -24.25);
	bounds.setLow(2, -504.855102539);
	setup.getStateSpace()->as<base::SE3StateSpace>()->setBounds(bounds);

	setup.setStartAndGoalStates(start, goal);

	// set Poisson-disk sampler
	//setup.getSpaceInformation()->setValidStateSamplerAllocator(&allocUniformStateSampler);
	setup.getSpaceInformation()->setValidStateSamplerAllocator(poisson::PoissonSampler::allocPoissonStateSampler);

	// set Poisson-disk planner
	//setup.setPlanner(base::PlannerPtr(new RRTIterationCount(setup.getSpaceInformation())));
	//setup.setPlanner(base::PlannerPtr(new pRRTIterationCount(setup.getSpaceInformation(), 8)));
	setup.setPlanner(base::PlannerPtr(new poisson::PoissonRRT(setup.getSpaceInformation()))); // todo: set parallel=8

	// attempt to solve the problem, and print it to screen if a solution is found
	if (setup.solve(30.0))
		setup.getSolutionPath().print(std::cout);

	return 0;
}
